/*
Copyright (C) 2021-2025 Casa Xu (also Zhiyan Xu) from HIT

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

/*
 * LOS_MagSen.c
 *
 *  Created on: 2023年2月17日
 *      Author: 64435
 */

#include "LOS_MagMe.h"

LOS_MAGMESTA_t STA_MAGME;

uint8_t TMPACK_MAGME[17] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

void LOS_MagMe_Init()    //不好用，别用了，懒得删，留着吧，拆成三个函数
{
    uint8_t *samplerate = "*99R=50\r\n";
    uint8_t *wen = "*99WE\r\n";
    uint8_t *format = "*99B\r\n";
    uint16_t i = 0;
    //gioSetBit(gioPORTB, 3, 1);
    LOS_MAX14830_ResetFIFO(MAGSEN_MAXNUM, MAGSEN_MAXPort);
    LOS_MAX14830_WriteBytes(MAGSEN_MAXNUM, MAGSEN_MAXPort, samplerate ,9); //修改采样频率为50Hz
    for (i=0;i<100;i++);
    LOS_MAX14830_ResetFIFO(MAGSEN_MAXNUM, MAGSEN_MAXPort);
    LOS_MAX14830_WriteBytes(MAGSEN_MAXNUM, MAGSEN_MAXPort, wen ,7);        //写使能
    for (i=0;i<1000;i++);
    LOS_MAX14830_ResetFIFO(MAGSEN_MAXNUM, MAGSEN_MAXPort);
    LOS_MAX14830_WriteBytes(MAGSEN_MAXNUM, MAGSEN_MAXPort, format ,6);     //输出模式切换至二进制
}

void LOS_MagMe_SetFreq()
{
    uint8_t *samplerate = "*99R=154\r\n";
    LOS_MAX14830_ResetFIFO(MAGSEN_MAXNUM, MAGSEN_MAXPort);
    LOS_MAX14830_WriteBytes(MAGSEN_MAXNUM, MAGSEN_MAXPort, samplerate ,9); //修改采样频率为50Hz
}

void LOS_MagMe_WE()
{
    uint8_t *wen = "*99WE\r\n";
    LOS_MAX14830_ResetFIFO(MAGSEN_MAXNUM, MAGSEN_MAXPort);
    LOS_MAX14830_WriteBytes(MAGSEN_MAXNUM, MAGSEN_MAXPort, wen ,7);        //写使能
}

void LOS_MagMe_Binary()
{
    uint8_t *format = "*99B\r\n";
    LOS_MAX14830_ResetFIFO(MAGSEN_MAXNUM, MAGSEN_MAXPort);
    LOS_MAX14830_WriteBytes(MAGSEN_MAXNUM, MAGSEN_MAXPort, format ,6);     //输出模式切换至二进制
}

void LOS_MagMe_REROFF()
{
    uint8_t *format = "*99N\r\n";
    LOS_MAX14830_ResetFIFO(MAGSEN_MAXNUM, MAGSEN_MAXPort);
    LOS_MAX14830_WriteBytes(MAGSEN_MAXNUM, MAGSEN_MAXPort, format ,6);     //关闭再进入回复(485通信时建议开启)
}

void LOS_MagMe_GetField()
{
    uint8_t *output = "*99P\r\n";
    uint32_t i = 0;
    LOS_MAX14830_ResetFIFO(MAGSEN_MAXNUM, MAGSEN_MAXPort);
    STA_MAGME.HL_TXF = 1;
    LOS_MAX14830_WriteBytes(MAGSEN_MAXNUM, MAGSEN_MAXPort, output ,6);     //向磁强计请求输出
}

void LOS_MagMe_GetField_Dyn()
{
    uint8_t msg = 0x33;
    LOS_CAN_SendPack(LOS_CAN_NUM1,0x33,&msg,1);
}

void LOS_MagMe_DataProcess(uint8_t* data)
{
    int16_t field_buf = 0;
    if (data[6] != 0x0D)
    {
        STA_MAGME.VAL = 0;
        STA_MAGME.Flag_D = 0;
    }
    else
    {
        memcpy(&field_buf, data, 2); 
        memcpy(&STA_MAGME.odata[0], &data[0], 2);             
        STA_MAGME.Field[0] = field_buf;
        STA_MAGME.Field[0] = STA_MAGME.Field[0]/15000 + FR_MAGCOMP[0];
        STA_MAGME.AfterComp[0] = STA_MAGME.Field[0]/15000 + FR_MAGCOMP[0];
        STA_MAGME.Field[0] = STA_MAGME.Field[0]/10000;   
        memcpy(&field_buf, &data[2], 2);
        memcpy(&STA_MAGME.odata[1], &data[2], 2);  
        STA_MAGME.Field[1] = field_buf;
        STA_MAGME.Field[1] = STA_MAGME.Field[1]/15000 + FR_MAGCOMP[1];
        STA_MAGME.AfterComp[1] = STA_MAGME.Field[1]/15000 + FR_MAGCOMP[1];
        STA_MAGME.Field[1] = STA_MAGME.Field[1]/10000;  
        memcpy(&field_buf, &data[4], 2);
        memcpy(&STA_MAGME.odata[2], &data[4], 2); 
        STA_MAGME.Field[2] = field_buf;
        STA_MAGME.Field[2] = STA_MAGME.Field[2]/15000 + FR_MAGCOMP[2];
        STA_MAGME.AfterComp[2] = STA_MAGME.Field[2]/15000 + FR_MAGCOMP[2];
        STA_MAGME.Field[2] = STA_MAGME.Field[2]/10000;  
        STA_MAGME.Flag_D = 1;
        STA_MAGME.VAL = 1;
        
        STA_MAGME.HL_TXF = 0;
        STA_MAGME.TMCNT++;
    }
}

void LOS_CAN_MagMeCallBack_Dyn(LOS_CANMSG_t* can_re)
{
    //首部校验用法：首部校验通过/未通过会改变canerrflag的值，所有消息会被先存在缓冲区，canerrflag有效时才会转存，无效时舍弃
    uint8_t id_filter[4];
    uint8_t sum = 0;
    uint8_t sum_check = 0;
    static uint8_t canerrflag = 0;         //表示首部校验是否通过，未通过：1，通过：0
    static uint8_t canmsgrec = 0xFF;       //消息类型（对应首部的rec）
    int16_t buf;

    memcpy(id_filter, &(can_re->ID), 4);   //将帧ID分为4个字节存储，便于处理
    if (id_filter[3]==0x00)                //若为首帧
    {
        sum = can_re->msg[0] + can_re->msg[1] + can_re->msg[2] + can_re->msg[3] + can_re->msg[4] + can_re->msg[5] + can_re->msg[6];
        sum_check = 0x100 - sum;
        if (sum_check == can_re->msg[7])   //校验和检查，消息有效性判断
        {
            canerrflag = 0;
        }
        else 
        {
           canerrflag = 1; 
        }

        if (canerrflag)
        {
            LOS_CAN_DoNothing();
        }
        else 
        {

        }
    }
    else if (id_filter[3]==0xFF)  //若为尾帧
    {
        if (canerrflag)
        {
            LOS_CAN_DoNothing();
        }
        else 
        {
            memcpy(&STA_MAGME.odata[0], &can_re->msg[0], 2); 
            memcpy(&buf, &can_re->msg[0], 2);
            STA_MAGME.Field[0] = ((double)buf * 0.0001)/15000;
            memcpy(&STA_MAGME.odata[1], &can_re->msg[2], 2);
            memcpy(&buf, &can_re->msg[2], 2);
            STA_MAGME.Field[1] = ((double)buf * 0.0001)/15000;
            memcpy(&STA_MAGME.odata[2], &can_re->msg[4], 2);
            memcpy(&buf, &can_re->msg[4], 2);
            STA_MAGME.Field[2] = ((double)buf * 0.0001)/15000;
            STA_MAGME.TMCNT++;
        }
        
        canerrflag = 0;   //最后一帧时清除标志位
        canmsgrec = 0xFF; //同上
    }
    else   //若为中间帧
    {
        if (canerrflag)
        {
            LOS_CAN_DoNothing();
        }
        else 
        {

        }
    }
}

void LOS_MagMe_PWROn()
{
    STA_MAGME.CMD = 1;
    STA_MAGME.First_Run = 0;
    LOS_EPS_TCSwitch(15, SWITCH_ON);
}

void LOS_MagMe_PWROff()
{
    STA_MAGME.CMD = 0;
    STA_MAGME.MSG = 1;
    LOS_EPS_TCSwitch(15, SWITCH_OFF);
}





